import cv2
from numpy import *
import functools

from scipy import optimize

from circle.method1 import get_edges_canny

src = cv2.imread(f'../resource/static/warmup/area/area1.bmp', cv2.IMREAD_COLOR)
gray = cv2.cvtColor(src, cv2.COLOR_BGR2GRAY)
# 应用高斯模糊来降噪
# blurred = cv2.GaussianBlur(gray, (5, 5), 0)
ret, image = cv2.threshold(gray, 200, 255, cv2.THRESH_BINARY)
edge_idx, edges = get_edges_canny(image)
# x = r_[36, 36, 19, 18, 33, 26]
# y = r_[14, 10, 28, 31, 18, 26]
x = edge_idx[1]
y = edge_idx[0]
# coordinates of the barycenter
x_m = mean(x)
y_m = mean(y)


def countcalls(fn):
    "decorator function count function calls "

    @functools.wraps(fn)
    def wrapped(*args):
        wrapped.ncalls += 1
        return fn(*args)

    wrapped.ncalls = 0
    return wrapped


# def f_3(beta, x):
#     """ implicit definition of the circle """
#     return (x[0] - beta[0]) ** 2 + (x[1] - beta[1]) ** 2 - beta[2] ** 2


# == METHOD 2b ==
# Advanced usage, with jacobian
method_2b = "leastsq with jacobian"


def calc_R(c):
    """ calculate the distance of each 2D points from the center c=(xc, yc) """
    return sqrt((x - c[0]) ** 2 + (y - c[1]) ** 2)


@countcalls
def f_2b(c):
    """ calculate the algebraic distance between the 2D points and the mean circle centered at c=(xc, yc) """
    Ri = calc_R(c)
    return Ri - Ri.mean()


@countcalls
def Df_2b(c):
    """ Jacobian of f_2b, with derivatives along the rows """
    xc, yc = c
    df2b_dc = empty((x.size, len(c)))

    Ri = calc_R(c).T
    df2b_dc[:, 0] = (xc - x.T) / Ri  # dR/dxc
    df2b_dc[:, 1] = (yc - y.T) / Ri  # dR/dyc
    df2b_dc = df2b_dc - df2b_dc.mean(axis=0)

    return df2b_dc


center_estimate = x_m, y_m
center_2b, ier = optimize.leastsq(f_2b, center_estimate, Dfun=Df_2b)

xc_2b, yc_2b = center_2b
Ri_2b = calc_R(center_2b)
R_2b = Ri_2b.mean()
residu_2b = sum((Ri_2b - R_2b) ** 2)
residu2_2b = sum((Ri_2b ** 2 - R_2b ** 2) ** 2)
ncalls_2b = f_2b.ncalls

print("\nMethod 2b :")
print("Functions calls : f_2b=%d Df_2b=%d" % (f_2b.ncalls, Df_2b.ncalls))
print(center_2b, ier, R_2b)
xc, yc = center_2b
r = R_2b
cv2.circle(src, (int(xc), int(yc)), int(r), (0, 255, 0), 2)
cv2.imshow("result", src)
cv2.waitKey(0)
